#include <ros/ros.h>

#include <uwb_pos/LinktrackTagframe0.h>
#include <uwb_pos/LinktrackNodeframe2.h>
#include <uwb_pos/LinktrackNode2.h>

#include <geometry_msgs/PoseStamped.h>
#include <nmea_msgs/Sentence.h>
#include <tf/tf.h>

#include <cmath>
#include <string>
#include <sstream>
#include <list>
#include <fstream>

#include <boost/algorithm/string.hpp>

#include "loc_core.h"

using std::list;
using uwb_pos::LinktrackNodeframe2;

// locations for uwb station
const double station_neu_base[2] = {758.395, 3833.955};
// double stations_neu[10 * 3] = {
//     758.395, 3833.955, 3.43,     // A0
//     745.346, 3839.027, 3.36,     // A1
//     734.702, 3837.705, 3.35,     // A2
//     741.549, 3804.622, 3.33,     // A3
//     730.905, 3780.613, 3.36,     // A4
//     727.683, 3836.525, 3.35,     // A5
//     750.565, 3834.409, 3.32,     // A6
//     729.749, 3817.450, 3.49,     // A7
//     769.085, 3842.232, 3.35,     // A8
//     760.813, 3845.606, 3.35      // A9
// };

const double stations_neu[10 * 3] = {
    0, 0.01, 3.43,          // A0  758.395, 3833.955
    -13.049, 5.072, 3.36,   // A1
    -23.693, 3.75, 3.35,    // A2
    -16.846, -29.333, 3.33, // A3
    -27.49, -53.342, 3.36,  // A4
    -30.712, 2.57, 3.35,    // A5
    -7.83, 0.454, 3.32,     // A6
    -28.646, -16.505, 3.49, // A7
    10.69, 8.277, 3.35,     // A8
    2.418, 11.651, 3.35     // A9
};

const double stations_gps_base[2] = {32.03102278, 118.93991417};
const double stations_gps[10 * 3] = {
    32.03102278, 118.93991417, 3.43, // A0
    32.03090556, 118.93996786, 3.36, // A1
    32.03080994, 118.93995380, 3.35, // A2
    32.03087144, 118.93960328, 3.33, // A3
    32.03077583, 118.93934883, 3.36, // A4
    32.03074688, 118.93994128, 3.35, // A5
    32.03095244, 118.93991894, 3.32, // A6
    32.03076544, 118.93973917, 3.49, // A7
    32.03111881, 118.94000192, 3.35, // A8
    32.03104450, 118.94003764, 3.35  // A9
};

const double areas_gps[2 * 4 * 2] = {
    32.03107600, 118.9399097138,
    32.03106231, 118.9400060727,
    32.03078080, 118.9399560604,
    32.03079469, 118.9398453771,

    32.03899542, 118.9394509983,
    32.03083356, 118.9399705600,
    32.03073436, 118.9399499278,
    32.03079096, 118.9394278311};

// clock-wise vertex list
const double areas_neu[2 * 4 * 2] = {
    764.320, 3833.532,
    762.795, 3842.627,
    731.458, 3837.919,
    733.004, 3827.472,

    744.676, 3790.250,
    737.332, 3839.285,
    726.289, 3837.342,
    732.589, 3788.068};

const int areas_vtx_n[2] = {4, 4};

const double areas_h[2] = {3.27, 3.27};

// area station mask
const int areas_mask[3 * 10] = {
    1, 1, 1, 0, 0, 1, 1, 0, 1, 1, // station mask for area 0
    1, 1, 1, 0, 0, 1, 1, 0, 1, 1, // station mask for area 1
    0, 0, 1, 1, 1, 1, 0, 1, 0, 0  // station mask for area 2
};

double dists[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

int dist_masks[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

ros::Time curr_stamp;
ros::Time last_stamp;

double last_east = 0.0;
double last_north = 0.0;

double gnss_yaw = 0.0;
double gnss_v = 0.0;
double gnss_east = 0.0;
double gnss_north = 0.0;
geometry_msgs::Quaternion gnss_quat;

std::list<double> err_list;
std::list<double> spd_list;
bool init_flag = false;
unsigned int uwb_count = 0;
unsigned int pose_count = 0;
unsigned int sten_count = 0;

void noopLoopCallback(const LinktrackNodeframe2 &msg)
{
    uwb_count++;
    last_stamp = curr_stamp;
    curr_stamp = ros::Time::now();
    memset(dist_masks, 0, sizeof(dist_masks));
    for (size_t idx = 0; idx != msg.nodes.size(); ++idx)
    {
        auto pos = msg.nodes[idx].id;
        dists[pos] = static_cast<double>(msg.nodes[idx].dis);
        // stations @ LOS mode and in 45 meters will be the valid station at receive
        std::isnan(dists[pos]) ? dist_masks[pos] = 0 : dist_masks[pos] = 1;
        ((dists[pos] > 55) || (dists[pos] <= 0.5)) ? dist_masks[pos] *= 0 : dist_masks[pos] *= 1;
        ((msg.nodes[idx].rx_rssi - msg.nodes[idx].fp_rssi) <= 15) ? dist_masks[pos] *= 1 : dist_masks[pos] *= 0;
    }
}

void gnssPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg)
{
    pose_count++;
    gnss_east = msg->pose.position.x;
    gnss_north = msg->pose.position.y;
    gnss_quat = msg->pose.orientation;
}

void gnssSentenceCallback(const nmea_msgs::SentenceConstPtr &msg)
{
    sten_count++;
    std::string msg_str = msg->sentence;
    std::vector<std::string> v_list;
    boost::split(v_list, msg_str, boost::is_any_of(","));
    gnss_yaw = std::stod(v_list[3]);
    gnss_v = std::stod(v_list[18]);
    // ROS_INFO("In sentence callback:gnss_yaw=%lf, gnss_ve=%lf, gnss_vn=%lf", gnss_yaw, gnss_ve, gnss_vn);
    // ROS_INFO("Senence: %s", msg_str.c_str());
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "uwb_pos");
    ros::NodeHandle nh;

    double baselink2uwb_x = 0.0;
    double baselink2uwb_y = 0.0;
    int n_stations = 0;
    int n_areas = 0;

    nh.param<double>("baselink2uwb_x", baselink2uwb_x, 0.79);
    nh.param<double>("baselink2uwb_y", baselink2uwb_y, 7.56);
    nh.param<int>("num_stations", n_stations, 10);
    nh.param<int>("num_areas", n_areas, 2);

    curr_stamp = ros::Time::now();

    ros::Subscriber nooploop_sub = nh.subscribe("/nlink_linktrack_nodeframe2", 10, noopLoopCallback);
    ros::Subscriber gnss_sub = nh.subscribe("/gnss_pose", 10, gnssPoseCallback);
    ros::Subscriber sentence_sub = nh.subscribe("/nmea_sentence", 10, gnssSentenceCallback);
    ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/uwb_pose", 10);

    init_senario(n_stations, stations_neu, areas_neu, areas_mask, station_neu_base, areas_h);

    ros::Rate loop_rate(20);

    geometry_msgs::PoseStamped pose;
    pose.header.frame_id = "map";

    // logging
    std::ofstream ofs_uwb;
    std::ofstream ofs_gnss;
    std::ofstream ofs_spd;
    ofs_uwb.open("/home/zcx/data_processing/xz/uwb.txt", std::ios::out);
    ofs_gnss.open("/home/zcx/data_processing/xz/gnss.txt", std::ios::out);
    ofs_spd.open("/home/zcx/data_processing/xz/spd.txt", std::ios::out);

    while (ros::ok())
    {
        ros::spinOnce();

        // make sure every thing initialized
        if (uwb_count < 3 || pose_count < 3 || sten_count < 3)
        {
            continue;
        }

        // fill in gnss msg as a backup plan
        pose.header.seq++;
        pose.header.stamp = curr_stamp;
        pose.pose.orientation = gnss_quat;
        pose.pose.position.z = 1;

        double east, north;
        int area = 0;

        // estimate the current state
        spd_list.push_back(gnss_v);
        if (spd_list.size() > 10)
            spd_list.pop_front();
        double velo = 0.0;
        for (auto itr = spd_list.begin(); itr != spd_list.end(); itr++)
            velo += *itr;
        velo /= spd_list.size();
        const double d_time = curr_stamp.toSec() - last_stamp.toSec();
        const double estim_d_step = velo * d_time;
        const double estim_d_east = velo * d_time * std::sin(gnss_yaw * M_PI / 180.0);
        const double estim_d_north = velo * d_time * std::cos(gnss_yaw * M_PI / 180.0);

        const double estim_east = last_east + estim_d_east;
        const double estim_north = last_north + estim_d_north;
        // ROS_INFO("Estimate_d_east=%lf, Estimate_d_north=%lf", estim_d_east, estim_d_north);

        bool valid = update_senario(dists, dist_masks, n_stations, gnss_east, gnss_north, &east, &north, &area);
        if (valid)
        {
            // calculate the uwb east and north, align to the GNSS frame
            const double delta_yaw = std::atan2(baselink2uwb_x, baselink2uwb_y);
            const double real_yaw = gnss_yaw / 180.0 * M_PI + delta_yaw;
            double baselink_len = std::hypot(baselink2uwb_y, baselink2uwb_x);
            east -= baselink_len * std::sin(real_yaw);
            north -= baselink_len * std::cos(real_yaw);

            //pre-set position type
            pose.pose.position.z = 2;

            // estimate current position w.r.t last one
            // ROS_INFO("last_north=%lf, north=%lf", last_north, north);
            double real_step = std::hypot(last_north - north, last_east - east);
            double cos_t = (estim_d_east * (east - last_east) + estim_d_north * (north - last_north)) / (estim_d_step * real_step);

            // detect an error position: step back or step to long
            if (cos_t <= 0.5)
            {
                // both angle and length are odd value, correct it using estimated position
                east = estim_east;
                north = estim_north;
                pose.pose.position.z = 4;
                ROS_INFO("ODD CORRECTION[tag=4]: cos_t=%lf, real_step=%lf, estim_d_step=%lf", cos_t, real_step, estim_d_step);
            }
            else
            {
                east = (estim_east + east) / 2;
                north = (estim_north + north) / 2;
                pose.pose.position.z = 3;
            }

            pose.pose.position.x = east;
            pose.pose.position.y = north;

            // estimate error w.r.t. gnss
            double abs_err = std::sqrt((east - gnss_east) * (east - gnss_east) + (north - gnss_north) * (north - gnss_north));
            err_list.push_front(abs_err);
            if (err_list.size() > 200)
                err_list.pop_back();

            double avg_err = 0.0;
            for (auto itr = err_list.begin(); itr != err_list.end(); ++itr)
                avg_err += *itr;
            avg_err /= err_list.size();
            ROS_INFO("ABS/AVG ERROR @AREA[%d] %.2lf / %.2lf", area, abs_err, avg_err);
        }
        else
        {
            // no valid position from uwb, use estimated pos instead
            pose.pose.position.x = estim_east;
            pose.pose.position.y = estim_north;
            pose.pose.position.z = 4;
            ROS_INFO("NO VALID UWB: use estimated position");
        }

        if (area == 0)
        {
            pose.pose.position.x = gnss_east;
            pose.pose.position.y = gnss_north;
            pose.pose.position.z = 1;
        }

        
        last_east = pose.pose.position.x;
        last_north = pose.pose.position.y;

        pose_pub.publish(pose);
        // logging
        ofs_uwb << pose.pose.position.y << '\t' << pose.pose.position.x << '\t' << area << '\t' << pose.pose.position.z << std::endl;
        ofs_gnss << gnss_north << '\t' << gnss_east << std::endl;
        ofs_spd << estim_d_north << '\t' << estim_d_east << '\t' << estim_d_step << '\t' << gnss_v << '\t' << velo << '\t' << gnss_yaw << std::endl;
        ofs_uwb.flush();
        ofs_gnss.flush();
        ofs_spd.flush();

        loop_rate.sleep();
    }

    return 0;
}